/**
 * @file app_motor.c
 * @brief 电机应用（前后、停止等操作），与上位机通过MODBUS协议进行通信
 * @author wangh (wanghuan3037@fiberhome.com)
 * @version 1.0
 * @date 2020-08-10
 * @copyright Copyright (c) 2020  烽火通信
 ***************************************************************
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version  <th>Author  <th>Description
 * <tr><td>2020-08-10 <td>1.0      <td>wangh   <td>内容
 * </table>
 */ 

#ifdef __cplusplus
extern "C" {
#endif

#include "project_config.h"
#if MOTOR_TASK_EN

#include "project_board.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include <string.h>
#include <stdlib.h>
#include "bsp_gpio.h"
#include "drv_motor.h"
#include "app_fifo.h"
#include "bsp_uart_v2.h"
#include "easyflash.h"


#undef LOG_LEVEL_
#define LOG_LEVEL_ 	LOG_LVL_DEBUG
#include "bsp_log.h"



/* motor实例初始化 */
MOTORS_CONTROL_S motor[] = {
	{ .index = 1, .Motor_ph1_pin = MOTOR1_PH1, .Motor_ph2_pin = MOTOR1_PH2, .Motor_set_pin = MOTOR_SET1, .M_sensor_int_pin = M1_SENSOR_INT, .location_key = "lc1", .group_value = 1, .group_key = "lg1" },
	{ .index = 2, .Motor_ph1_pin = MOTOR2_PH1, .Motor_ph2_pin = MOTOR2_PH2, .Motor_set_pin = MOTOR_SET2, .M_sensor_int_pin = M2_SENSOR_INT, .location_key = "lc2", .group_value = 1, .group_key = "lg2" },
	{ .index = 3, .Motor_ph1_pin = MOTOR3_PH1, .Motor_ph2_pin = MOTOR3_PH2, .Motor_set_pin = MOTOR_SET3, .M_sensor_int_pin = M3_SENSOR_INT, .location_key = "lc3", .group_value = 0, .group_key = "lg3" },
	{ .index = 4, .Motor_ph1_pin = MOTOR4_PH1, .Motor_ph2_pin = MOTOR4_PH2, .Motor_set_pin = MOTOR_SET4, .M_sensor_int_pin = M4_SENSOR_INT, .location_key = "lc4", .group_value = 0, .group_key = "lg4" },
	{ .index = 5, .Motor_ph1_pin = MOTOR5_PH1, .Motor_ph2_pin = MOTOR5_PH2, .Motor_set_pin = MOTOR_SET5, .M_sensor_int_pin = M5_SENSOR_INT, .location_key = "lc5", .group_value = 0, .group_key = "lg5" },
	{ .index = 6, .Motor_ph1_pin = MOTOR6_PH1, .Motor_ph2_pin = MOTOR6_PH2, .Motor_set_pin = MOTOR_SET6, .M_sensor_int_pin = M6_SENSOR_INT, .location_key = "lc6", .group_value = 0, .group_key = "lg6" },
	{ .index = 7, .Motor_ph1_pin = MOTOR7_PH1, .Motor_ph2_pin = MOTOR7_PH2, .Motor_set_pin = MOTOR_SET7, .M_sensor_int_pin = M7_SENSOR_INT, .location_key = "lc7", .group_value = 0, .group_key = "lg7" },
	{ .index = 8, .Motor_ph1_pin = MOTOR8_PH1, .Motor_ph2_pin = MOTOR8_PH2, .Motor_set_pin = MOTOR_SET8, .M_sensor_int_pin = M8_SENSOR_INT, .location_key = "lc8", .group_value = 2, .group_key = "lg8" },
	{ .index = 9, .Motor_ph1_pin = MOTOR9_PH1, .Motor_ph2_pin = MOTOR9_PH2, .Motor_set_pin = MOTOR_SET9, .M_sensor_int_pin = M9_SENSOR_INT, .location_key = "lc9", .group_value = 2, .group_key = "lg9" },
	{ .index = 10, .Motor_ph1_pin = MOTOR10_PH1, .Motor_ph2_pin = MOTOR10_PH2, .Motor_set_pin = MOTOR_SET10, .M_sensor_int_pin = M10_SENSOR_INT, .location_key = "lc10", .group_value = 0, .group_key = "lg10" },
	
};

tMotorDriver *mtrDriver = NULL;	/* motor 驱动 */


#if USE_BSP_BUTTON
/**
 * @brief 按键事件处理函数（电机传感输入）
 * @param  pin_no           按键号
 * @param  btn_action      	按键动作（BUTTON_PUSH/BUTTON_RELEASE）
 */
static void btn_event_handle(uint8_t pin_no, uint8_t btn_action)
{
	if(btn_action == BUTTON_PUSH)
		log_d("btn_event_handle %d BUTTON_PUSH\r\n", pin_no);
	else if(btn_action == BUTTON_RELEASE)
		log_d("btn_event_handle %d BUTTON_RELEASE\r\n", pin_no);
	
	if(mtrDriver != NULL){
	switch(pin_no)
	{
		case 1:{	/* M6_SENSOR_INT_Pin */
			log_d("M6_SENSOR_INT_Pin int %d\r\n", btn_action);
			mtrDriver->sensor_handle(&motor[5]);
		}break;
		case 2:{	/* M7_SENSOR_INT_Pin */
			log_d("M7_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[6]);
		}break;
		case 3:{	/* M10_SENSOR_INT_Pin */
			log_d("M10_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[9]);
		}break;
		case 4:{	/* M8_SENSOR_INT_Pin */
			log_d("M8_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[7]);
		}break;
		case 5:{
			log_d("M9_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[8]);
		}break;
		case 6:{
			log_d("M3_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[2]);
		}break;
		case 9:{
			log_d("M4_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[3]);
		}break;
		case 10:{
			log_d("M1_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[0]);
		}break;
		case 11:{
			log_d("M5_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[4]);
		}break;
		case 13:{
			log_d("M2_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[1]);
		}break;
		default:
			log_w("EXTI_Callback get error:%d\n", pin_no);
		break;
	}
	}
}

#else
/**
 * @brief IO中断回调函数
 * @param  GPIO_Pin	引脚编号
 */
static void GPIO_EXTI_Callback(uint8_t GPIO_Pin)
{
	if(mtrDriver != NULL){
	switch(GPIO_Pin)
	{
		case 1:{	/* M6_SENSOR_INT_Pin */
			log_d("M6_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[5]);
		}break;
		case 2:{	/* M7_SENSOR_INT_Pin */
			log_d("M7_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[6]);
		}break;
		case 3:{	/* M10_SENSOR_INT_Pin */
			log_d("M10_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[9]);
		}break;
		case 4:{	/* M8_SENSOR_INT_Pin */
			log_d("M8_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[7]);
		}break;
		case 5:{
			log_d("M9_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[8]);
		}break;
		case 6:{
			log_d("M3_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[2]);
		}break;
		case 9:{
			log_d("M4_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[3]);
		}break;
		case 10:{
			log_d("M1_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[0]);
		}break;
		case 11:{
			log_d("M5_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[4]);
		}break;
		case 13:{
			log_d("M2_SENSOR_INT_Pin int\r\n");
			mtrDriver->sensor_handle(&motor[1]);
		}break;
		default:
			log_w("EXTI_Callback get error:%d\n", GPIO_Pin);
		break;
	}
	}
}
#endif


/**
 * @brief 按键轮询任务
 */
void key_task(void *para)
{
	Gpio_t key_foward, key_back, key_stop, key_reset;
	
	GpioInit( &key_foward, Motor_Forward, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	GpioInit( &key_back, Motor_Reverse, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	GpioInit( &key_stop, Motor_Break, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	GpioInit( &key_reset, Motor_Reset, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	
	uint8_t motor_num = sizeof(motor)/sizeof(MOTORS_CONTROL_S);
	
	
	/* 一次只能执行一个按键（不支持连按），复位按键执行期间其他按键暂时失效 */
	while(1)
	{
		if( GpioRead(&key_foward) == 0 )
		{
			vTaskDelay(KEY_DELAY_TIME);
			if( GpioRead(&key_foward) == 0 )
			{
				while( GpioRead(&key_foward) == 0 );			/* 按键弹起有效 */
				log_d("key_foward \r\n");
				for(uint8_t i=0; i<motor_num; i++)
				{
					if( GpioRead(&motor[i].Motor_set) == 0 )	/* 拨码只对按键操作有效 */
						mtrDriver->move( &motor[i], M_FORWARD );
				}
			}
		}
		
		if( GpioRead(&key_back) == 0 )
		{
			vTaskDelay(KEY_DELAY_TIME);
			if( GpioRead(&key_back) == 0 )
			{
				while( GpioRead(&key_back) == 0 );
				log_d("key_back \r\n");
				for(uint8_t i=0; i<motor_num; i++)
				{
					if( GpioRead(&motor[i].Motor_set) == 0 )
						mtrDriver->move( &motor[i], M_BACK );
				}
			}
		}
		
		if( GpioRead(&key_stop) == 0 )
		{
			vTaskDelay(KEY_DELAY_TIME);
			if( GpioRead(&key_stop) == 0 )
			{
				while( GpioRead(&key_stop) == 0 );
				log_d("key_stop \r\n");
				for(uint8_t i=0; i<motor_num; i++)
				{
					if( GpioRead(&motor[i].Motor_set) == 0 )
						mtrDriver->move( &motor[i], M_STOP );
				}
			}
		}
		
		if( GpioRead(&key_reset) == 0 )
		{
			vTaskDelay(KEY_DELAY_TIME);
			if( GpioRead(&key_reset) == 0 )
			{
				while( GpioRead(&key_reset) == 0 );
				log_d("key_reset \r\n");
				for(uint8_t i=0; i<motor_num; i++)
				{
					if( GpioRead(&motor[i].Motor_set) == 0 )
						mtrDriver->move( &motor[i], M_RESET );
				}
			}
		}
		
		vTaskDelay(10);
	}
}


#if 1
/* motor MODBUS 通信 */
Gpio_t rs485_dir;


/**
 * @brief 接收帧结构
 */
typedef struct sMBFRM
{
	uint8_t addr;
    uint8_t cmd;
    uint16_t addr_start;
	uint16_t addr_len;
	uint16_t crc;
	
}tMBFRM;

/**
 * @brief 帧错误类型
 */
typedef enum {
	MB_OK = 0,
	MB_ERR_LEN = 1,
	MB_ERR_ADDR,
	MB_ERR_CRC,
	MB_ERR_CMD,
	MB_ERR_DADDR,
	MB_ERR_DADDRLEN,
	
}MB_ERRCODES;


/**
 * @brief crc16计算
 */
static unsigned short calc_CRC(unsigned char *str, int num)
{
	unsigned char *addr = str;
    unsigned short crc = 0xFFFF;
    int i;
    while (num--)
	{
        crc ^= *addr++;
        for (i = 0; i < 8; i++)
        {crc =(crc & 0x0001) ? ((crc>>1)^0xA001) : (crc>>1);}
    }
	
	return crc;
}

/**
 * @brief modbus接收帧检查
 * @param  buffer       	接收数据
 * @param  buf_len         	接收数据长度
 * @param  pfrm            	输出帧结构体
 * @return uint8_t 帧错误类型
 */
static uint8_t mb_frm_check(uint8_t *buffer, uint16_t buf_len, tMBFRM *pfrm)
{
	if(buf_len < 8) 
	{
		log_v("mb_frm MB_ERR_LEN\r\n");
		return MB_ERR_LEN;
	}
	
	pfrm->addr = buffer[0];
	if( pfrm->addr != MOTOR_ADDR ) return MB_ERR_ADDR;
	
	pfrm->crc = (uint16_t)(buffer[buf_len-1]<<8) + buffer[buf_len-2];
	uint16_t crc1 = calc_CRC(buffer, buf_len-2);
	if( pfrm->crc !=  crc1 ) 	/* CRC异常 */
	{
		log_d("mb_frm MB_ERR_CRC %04X %04X\r\n", pfrm->crc, crc1);
		return  MB_ERR_CRC;
	}
	
	pfrm->cmd = buffer[1];
	if( (pfrm->cmd != 0x03) && (pfrm->cmd != 0x10) && (pfrm->cmd != 0x06) )	/* 指令异常 */
		return MB_ERR_CMD;
	
	pfrm->addr_start = (uint16_t)(buffer[3]<<8) + buffer[2];
	if( (pfrm->addr_start < 1) || ((pfrm->addr_start > 0x0002)&&(pfrm->addr_start < 0x1001)) || ((pfrm->addr_start > 0x1009)&&(pfrm->addr_start < 0x2001)) || (pfrm->addr_start > 0x2009) )	/* 寄存器地址异常 */
		return MB_ERR_DADDR;
	
	pfrm->addr_len = (uint16_t)(buffer[5]<<8) + buffer[4];
	if(pfrm->addr_len > MAX_REG_NUM)	/* 寄存器梳理异常 */
		return MB_ERR_DADDRLEN;
	uint16_t addr1 = pfrm->addr_start + pfrm->addr_len;	/* 稽查寄存器地址是否超出 */
	if( (addr1 < 1) || ((addr1 > 0x0002)&&(addr1 < 0x1001)) || ((addr1 > 0x1009)&&(addr1 < 0x2001)) || (addr1 > 0x2009) )
		return MB_ERR_DADDRLEN;
	
    return MB_OK;
}

/**
 * @brief RS485发送
 * @param  buf     		发送数据
 * @param  buf_len    	发送数据长度
 */
static void mb_send(uint8_t *buf, uint16_t buf_len)
{
	log_d("mb_send:");
	log_hexdump(buf, buf_len);
	
	GpioWrite( &rs485_dir, 1 );
	UartSend( UART_2, buf, buf_len );
	GpioWrite( &rs485_dir, 0 );
}

/**
 * @brief 异常应答
 * @param  pfrm     	接收帧
 * @param  errcode 		错误码
 */
static void mb_sentACK( tMBFRM *pfrm, uint8_t errcode)
{
    uint8_t sbuf[5];
	
	sbuf[0] = pfrm->addr;
	sbuf[1] = pfrm->cmd + 128;
	sbuf[2] = errcode;
	
	uint16_t crc1 = calc_CRC(sbuf, 3);
	sbuf[3] = (uint8_t)(crc1 &0x00FF);
	sbuf[4] = (uint8_t)(crc1>>8);
	
	mb_send(sbuf, 5);
}

/**
 * @brief 回应 读保持寄存器，命令0X03
 * @param  buf   	接收帧数据
 * @param  pfrm  	接收帧结构
 */
static void mb_sentfor_readReg( tMBFRM *pfrm )
{
	uint8_t sbufLen = pfrm->addr_len * 2 + 5;
	uint8_t sbuf[sbufLen];
	
	sbuf[0] = pfrm->addr;
	sbuf[1] = pfrm->cmd;
	sbuf[2] = pfrm->addr_len * 2;
	
	if(pfrm->addr_start>>8 == 0x00)	/* 版本寄存器 */
	{
		if(pfrm->addr_start == 0x0001)
		{
			sbuf[3] = (uint8_t)(MOTOR_SW_VERSION&0x00FF);
			sbuf[4] = (uint8_t)(MOTOR_SW_VERSION>>8);
			if(pfrm->addr_len == 2)
			{
				sbuf[5] = (uint8_t)(MOTOR_HW_VERSION&0x00FF);
				sbuf[6] = (uint8_t)(MOTOR_HW_VERSION>>8);
			}
		}
		else if(pfrm->addr_start == 0x0002)
		{
			sbuf[3] = (uint8_t)(MOTOR_HW_VERSION&0x00FF);
			sbuf[4] = (uint8_t)(MOTOR_HW_VERSION>>8);
		}
	}
	else if(pfrm->addr_start>>8 == 0x10)/* 不支持 */
	{
		mb_sentACK(pfrm, 2);
		return;
	}
	else if(pfrm->addr_start>>8 == 0x20)
	{
		for(uint8_t i=(pfrm->addr_start-0x2001); i<(pfrm->addr_start-0x2001+pfrm->addr_len); i++)
		{
			sbuf[3+(i-(pfrm->addr_start-0x2001))*2] = motor[i].location_value;
			sbuf[4+(i-(pfrm->addr_start-0x2001))*2] = 0x00;
		}
	}
	
	uint16_t crc1 = calc_CRC(sbuf, sbufLen-2);
	sbuf[sbufLen-2] = (uint8_t)(crc1&0x00FF);
	sbuf[sbufLen-1] = (uint8_t)(crc1>>8);
	
	mb_send(sbuf, sbufLen);
}

/**
 * @brief 回应 写多个保持寄存器，命令0X10
 * @param  buf   	接收帧数据
 * @param  pfrm  	接收帧结构
 */
static void mb_sentfor_writeMultReg( uint8_t *buf, tMBFRM *pfrm )
{
	if(pfrm->addr_start>>8 == 0x00)			/* 不支持写 */
	{
		mb_sentACK(pfrm, 2);
		return;
	}
	else if(pfrm->addr_start>>8 == 0x10)	/* 寄存器不支持 */
	{
		mb_sentACK(pfrm, 2);
		return;
	}
	else if(pfrm->addr_start>>8 == 0x20)
	{
		for(uint8_t i=(pfrm->addr_start-0x2001); i<(pfrm->addr_start-0x2001+pfrm->addr_len); i++)
		{
			uint8_t data = buf[7+(i-(pfrm->addr_start-0x2001))*2];
			if(data > 3)
			{
				mb_sentACK(pfrm, 3);	/* 数据错误 */
				return;
			}
			mtrDriver->move(&motor[i], (MOTOR_ACTION)data);
		}
	}
	
	uint8_t sbuf[8];
	sbuf[0] = pfrm->addr;
	sbuf[1] = pfrm->cmd;
	sbuf[2] = (uint8_t)(pfrm->addr_start&0x00FF);
	sbuf[3] = (uint8_t)(pfrm->addr_start>>8);
	sbuf[4] = (uint8_t)(pfrm->addr_len&0x00FF);
	sbuf[5] = (uint8_t)(pfrm->addr_len>>8);
	
	uint16_t crc1 = calc_CRC(sbuf, 6);
	sbuf[6] = (uint8_t)(crc1&0x00FF);
	sbuf[7] = (uint8_t)(crc1>>8);
	
	mb_send(sbuf, 8);
}

/**
 * @brief 回应 写单个保持寄存器，命令0X06
 * @param  buf   	接收帧数据
 * @param  pfrm  	接收帧结构
 */
static void mb_sentfor_writeOneReg( uint8_t *buf, tMBFRM *pfrm )
{
	if(pfrm->addr_start>>8 == 0x00)			/* 不支持写 */
	{
		mb_sentACK(pfrm, 2);
		return;
	}
	else if(pfrm->addr_start>>8 == 0x10)	/* 寄存器不支持 */
	{
		mb_sentACK(pfrm, 2);
		return;
	}
	else if(pfrm->addr_start>>8 == 0x20)
	{
		if(buf[4] > 3)
		{
			mb_sentACK(pfrm, 3);	/* 数据错误 */
			return;
		}
		mtrDriver->move(&motor[pfrm->addr_start-0x2001], (MOTOR_ACTION)buf[4]);
	}
	
	mb_send(buf, 8);	/* 直接应答本帧 */
}

/**
 * @brief Modbus帧处理函数
 * @param  buffer    	原始帧数据
 * @param  buf_len     	原始帧数据长度
 */
static void mb_frm_handle(uint8_t *buffer, uint16_t buf_len)
{
	tMBFRM mb_frm;
	
	log_d("mb_frm recv:");
	log_hexdump(buffer, buf_len);
	
	uint8_t ret = mb_frm_check(buffer, buf_len, &mb_frm);
	if(ret > 0)
	{
		log_e("mb_frm_check error %d\r\n", ret);
		if(ret == MB_ERR_CMD)
			mb_sentACK(&mb_frm, 1);	/* 非法功能码 */
		else if( (ret == MB_ERR_DADDR) || (ret == MB_ERR_DADDRLEN) )
			mb_sentACK(&mb_frm, 2);	/* 非法数据地址 */
		
		return;
	}
	
	if(mb_frm.cmd == 0x03)		/* 读操作 */
	{
		mb_sentfor_readReg(&mb_frm);
	}
	else if(mb_frm.cmd == 0x10)	/* 写多个寄存器命令 */
	{
		mb_sentfor_writeMultReg(buffer, &mb_frm);
	}
	else if(mb_frm.cmd == 0x06)	/* 写单个寄存器命令 */
	{
		mb_sentfor_writeOneReg(buffer, &mb_frm);
	}
	

}


/**
 * @brief 485通信任务
 */
void rs485_task(void *para)
{
	log_i("rs485_task start... \r\n");
	
	UartInit( UART_2, RS485_TXD, RS485_RXD, 64 );
	UartConfig( UART_2, RX_TX, 115200, UART_8_BIT, UART_1_STOP_BIT, NO_PARITY, NO_FLOW_CTRL );
	GpioInit( &rs485_dir, RS485_DIR, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
	
	log_i("rs485 init ok... \r\n");
	
	while(1)
    {
		uint8_t buffer[129];
        memset(buffer, 0, 129);
        int32_t recv_len = UartRecv( UART_2, buffer, 128, portMAX_DELAY );
        if( recv_len > 0)
        {
            log_d("485 recv(%d):\r\n", recv_len);
            log_hexdump(buffer, recv_len);
			mb_frm_handle(buffer, recv_len);
        }
        else
        {
            log_v("uart recv timeout\r\n");
        }
        
        vTaskDelay(1);
    }
	
}
#endif



static uint8_t motor_selftest_en = 0;	/* 电机自动测试使能 */
/**
 * @brief 电机任务
 */
void motor_task(void *para)
{
	uint8_t i;
	
	uint8_t motor_num = sizeof(motor)/sizeof(MOTORS_CONTROL_S);
	log_d("board drv %d motors\r\n", motor_num);
	
	mtrDriver = MotorDriverInit(motor, motor_num);	/* motor驱动句柄初始化 */
	
	for(i=0; i<motor_num; i++)
	{
		ef_get_env_blob(motor[i].location_key, &motor[i].location_value, 1 , NULL);	/* 开机获取flash存储的位置信息 */
		ef_get_env_blob(motor[i].group_key, &motor[i].group_value, 1 , NULL);		/* 开机获取flash存储的组号信息 */
		log_d("motor%d location_value = %d  group_value = %d\r\n", i+1, motor[i].location_value, motor[i].group_value);
		
		#if USE_BSP_BUTTON
		mtrDriver->Init( &motor[i], btn_event_handle );	/* motor实例初始化、回调注册 */
		#else
		mtrDriver->Init( &motor[i], GPIO_EXTI_Callback );
		#endif
	}
	
	mtrDriver->create_move_task();
	
	/* 按键任务 */
	xTaskCreate( key_task, "key_task", 128, NULL, MOTOR_TASK_PRIO+1, NULL);
	
	/* 485通信 */
	xTaskCreate( rs485_task, "rs485_task", 256, NULL, MOTOR_TASK_PRIO+2, NULL);
	
//	vTaskDelete( NULL );	/* 退出任务 */
	while(1)
	{
		if(motor_selftest_en == 1)	/* 自动前后移动测试 */
		{
			log_d("test foward \r\n");
			for(i=0; i<motor_num; i++)
			{
				if( GpioRead(&motor[i].Motor_set) == 0 )	/* 拨码只对按键操作有效 */
					mtrDriver->move( &motor[i], M_FORWARD );
			}
			vTaskDelay(MAX_MOVE_TIME);
			log_d("test back \r\n");
			for(i=0; i<motor_num; i++)
			{
				if( GpioRead(&motor[i].Motor_set) == 0 )	/* 拨码只对按键操作有效 */
					mtrDriver->move( &motor[i], M_BACK );
			}
			vTaskDelay(MAX_MOVE_TIME);
		}
		
		vTaskDelay(10);
	}
}

void application_start(void)
{
	xTaskCreate((TaskFunction_t )motor_task,     
                (const char*    )"motor_task",   
                (uint16_t       )MOTOR_STK_SIZE, 
                (void*          )NULL,
                (UBaseType_t    )MOTOR_TASK_PRIO,
                (TaskHandle_t*  )NULL);
}



#if SHELL_TASK_EN
#include "nr_micro_shell.h"
#include "bsp_delay.h"
/**
 * @brief 电机测试指令
 */
void motor_test_cmd(char argc, char *argv)
{
	uint8_t motor_index;
	
	if (argc > 2)
	{
		if (!strcmp("help", &argv[argv[1]]))
		{
			shell_printf("motor support cmd:\r\n");
			shell_printf("forward back stop reset (motor forward 1 2 5 9)\r\n");
			shell_printf("group: set/reset the flash group (motor group set 1 1 2 3)\r\n");
			shell_printf("lreset: reset the flash location to unkown (motor lreset 11)\r\n");
			shell_printf("selftest: the motor auto selftest (motor selftest on/off)\r\n");
			shell_printf("show: show motor info (motor show 11)\r\n");
		}

		else if (!strcmp("reset", &argv[argv[1]]))
		{
			for(uint8_t i=2; i<argc; i++)
			{
				motor_index = (uint8_t)atoi(&argv[argv[i]]) - 1;
				if(motor_index > 9)
				{
					shell_printf("motor index arguments is error(motor index < 9)!\r\n");
					continue;
				}
				mtrDriver->move( &motor[motor_index], M_RESET );
			}
		}

		else if (!strcmp("forward", &argv[argv[1]]))
		{
			for(uint8_t i=2; i<argc; i++)
			{
				motor_index = (uint8_t)atoi(&argv[argv[i]]) - 1;
				if(motor_index > 9)
				{
					shell_printf("motor index arguments is error(motor index < 9)!\r\n");
					continue;
				}
				mtrDriver->move( &motor[motor_index], M_FORWARD );
			}
		}

		else if (!strcmp("back", &argv[argv[1]]))
		{
			for(uint8_t i=2; i<argc; i++)
			{
				motor_index = (uint8_t)atoi(&argv[argv[i]]) - 1;
				if(motor_index > 9)
				{
					shell_printf("motor index arguments is error(motor index < 9)!\r\n");
					continue;
				}
				mtrDriver->move( &motor[motor_index], M_BACK );
			}
		}

		else if (!strcmp("stop", &argv[argv[1]]))
		{
			for(uint8_t i=2; i<argc; i++)
			{
				motor_index = (uint8_t)atoi(&argv[argv[i]]) - 1;
				if(motor_index > 9)
				{
					shell_printf("motor index arguments is error(motor index < 9)!\r\n");
					continue;
				}
				mtrDriver->move( &motor[motor_index], M_STOP );
			}
		}
		
		else if (!strcmp("group", &argv[argv[1]]))
		{
			if (!strcmp("set", &argv[argv[2]]))
			{
				if(argc < 5)
				{
					shell_printf("motor group set need more arguments...\r\n");
					return;
				}
				else if(argc > MAX_MOTOR_START_NUM+4)
				{
					shell_printf("motor group max motor num is %d\r\n", MAX_MOTOR_START_NUM);
					return;
				}
				
				uint8_t tmp_group = (uint8_t)atoi(&argv[argv[3]]);

				uint8_t tmp_group_num = argc - 4;
				for(uint8_t i=0; i<sizeof(motor)/sizeof(MOTORS_CONTROL_S); i++)
				{
					if(motor[i].group_value == tmp_group)
						tmp_group_num++;
				}
				if(tmp_group_num > MAX_MOTOR_START_NUM)
				{
					shell_printf("motor group motor num is exceed\r\n");
					return;
				}

				for(uint8_t i=4; i<argc; i++)
				{
					uint8_t tmp_index = (uint8_t)atoi(&argv[argv[i]]) - 1;
					if(motor[tmp_index].group_value != tmp_group)
					{
						motor[tmp_index].group_value = tmp_group;
						ef_set_env_blob(motor[tmp_index].group_key, &tmp_group, 1);
					}
				}
			}
			else if (!strcmp("reset", &argv[argv[2]]))	/* 分组全部清空 */
			{
				for(uint8_t i=0; i<sizeof(motor)/sizeof(MOTORS_CONTROL_S); i++)
				{
					if(motor[i].group_value > 0)
					{
						motor[i].group_value = 0;
						ef_set_env_blob(motor[i].group_key, &motor[i].group_value, 1);
					}
				}
			}
			shell_printf("motor group set ok, use motor show to see the result\r\n");
		}

		else if (!strcmp("lreset", &argv[argv[1]]))		/* flash位置重置(设置单个 1~10，设置全部 >10) */
		{
			motor_index = (uint8_t)atoi(&argv[argv[2]]) - 1;
			if(motor_index > 9)
			{
				for(uint8_t i=0; i<sizeof(motor)/sizeof(MOTORS_CONTROL_S); i++)
				{
					motor[i].location_value = M_UNKNOWN;
					ef_set_env_blob(motor[i].location_key, &motor[i].location_value, 1);
				}
			}
			else
			{
				motor[motor_index].location_value = M_UNKNOWN;
				ef_set_env_blob(motor[motor_index].location_key, &motor[motor_index].location_value, 1);
			}
			shell_printf("motor location reset ok, use motor show to see the result\r\n");
		}

		else if (!strcmp("selftest", &argv[argv[1]]))	/* 电机自动测试 */
		{
			if (!strcmp("on", &argv[argv[2]]))
			{
				motor_selftest_en = 1;
			}
			else if (!strcmp("off", &argv[argv[2]]))
			{
				motor_selftest_en = 0;
			}
		}

		else if (!strcmp("show", &argv[argv[1]]))		/* 电机信息显示 */
		{
			motor_index = (uint8_t)atoi(&argv[argv[2]]) - 1;
			if(motor_index > 9)
			{
				for(uint8_t i=0; i<sizeof(motor)/sizeof(MOTORS_CONTROL_S); i++)
				{
					shell_printf("motor%d location_value = %d  group_value = %d\r\n", i+1, motor[i].location_value, motor[i].group_value);
				}
			}
			else
			{
				shell_printf("motor%d location_value = %d  group_value = %d\r\n", motor_index+1, motor[motor_index].location_value, motor[motor_index].group_value);
			}
		}

		else if (!strcmp("delay", &argv[argv[1]]))	//延时精度测试 motor delay 5530 100
		{
			uint32_t xStart = xTaskGetTickCount();
			vTaskDelay(100);
			shell_printf("100ms = %d\r\n", xTaskGetTickCount()-xStart);
		}

		else
		{
			shell_printf("unsupport cmd\r\n");
		}
	}
	else
	{
		shell_printf("motor need more arguments!\r\n");
	}
	
}

NR_SHELL_CMD_EXPORT(motor, motor_test_cmd);

#endif	/* SHELL_TASK_EN */


#endif	/* MOTOR_TASK_EN */

#ifdef __cplusplus
}
#endif //__cplusplus 

/*********** (C) COPYRIGHT 2020 FiberHome *****END OF FILE****/
